#ifndef UTILITY_HPP
#define UTILITY_HPP

#include <stdlib.h>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <sstream>
#include <vector>
#include <list>
#include <chrono>
#include <memory>
#include <algorithm> 
#include <stdexcept> 

// #include <boost/any.hpp>
// #include <boost/algorithm/string.hpp>
// #include <omp.h>
#include <limits>
#include "parallel_utility.hpp"


#include "opencv2/opencv_modules.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/opencv.hpp"
#include "opencv2/video/tracking.hpp"
#include "opencv2/features2d/features2d.hpp"

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <base/common.h>

//import lib
#include <log_stream.hpp>

//import LSD
extern "C"
{
    #include <lsd.h>
};


namespace LfUtils{

// example usage
// static auto LOG = spdlog::stdout_color_mt("${NANE}");
#define LOG_INFO  spdlog_API::LogStream::logInfo(LOG)
#define LOG_WARN  spdlog_API::LogStream::logWarn(LOG)
#define LOG_ERROR spdlog_API::LogStream::logError(LOG)
#define LOG_DEBUG spdlog_API::LogStream::logDebug(LOG)
//#define THROW_EXCEPTION spdlog_API::LogStream::throwExeption()<<__func__<<'('<<__FILE__<<"): "


#define CODE_POS "...@" << __func__ << " in " << __FILE__  << ":" <<  __LINE__ << std::endl;
#define LOG_MSG(msg)          {std::cerr << "[Log]: " << msg << CODE_POS;}
#define FINISH_LOG_MSG(msg)   {std::cerr << "[Log]: finish " << msg << "..." << std::endl;}
#define ERROR_MSG(msg)        {std::cerr << "[Error]: " << msg << CODE_POS;}
#define WARNING_MSG(msg)      {std::cerr << "[WARNING]: " << msg << CODE_POS;}                                                    
#define ERROR_MSG_EXIT(msg)   {std::stringstream ss; ss << "[Error]: " << msg << CODE_POS;   throw std::runtime_error(ss.str());}
#define WARNING_MSG_EXIT(msg) {std::stringstream ss; ss << "[WARNING]: " << msg << CODE_POS; throw std::runtime_error(ss.str());}                                                    
#define FORBIDDEN_FUNC  ERROR_MSG_EXIT("Using forbidden function")
#define OFFLINE_ONLY    ERROR_MSG_EXIT("For OFFLINE mode only")
#define ONVEHICLE_ONLY  ERROR_MSG_EXIT("For ONVEHICLE mode only")


#define CV_AA cv::LINE_AA
#define CV_BGR2GRAY cv::COLOR_BGR2GRAY

const cv::Vec3b RED(0,0,255);
const cv::Vec3b BLUE(255, 0, 0);
const cv::Vec3b GREEN(0,255,0);
const cv::Vec3b YELLOW(0,255,255);
const cv::Vec3b WHITE(255, 255, 255);
const cv::Vec3b BLACK(0, 0, 0);
const cv::Vec3b PURPLE(240, 32, 160);
const cv::Vec3b MAGENTA(255, 0, 255);
const cv::Vec3b GREY(192,192,192);

const float DegperRad = 180.0/M_PI;
const float RadperDeg = M_PI/180.0;
const float cmTOm = 1.0/100;
const float mTOcm = 100;
const float sqrt2pi = std::sqrt(2*M_PI);


void LSD_LineDetect(cv::Mat image, double thLength, std::vector<std::vector<double>> &lines, double scale = 0.8, const cv::Rect& ROI = cv::Rect(0,0,0,0));

template <typename T>
inline float euclideanDist(const T& p, const T& q) 
{
    T diff = p - q;
    return std::sqrt(diff.x*diff.x + diff.y*diff.y);
}

template <typename T>
inline T center_diff(const cv::Rect_<T> &lhs, const cv::Rect_<T> &rhs) 
{
    auto lhs_center = (lhs.tl() + lhs.br()) / 2;
    auto rhs_center = (rhs.tl() + rhs.br()) / 2;
    return euclideanDist(lhs_center, rhs_center);
}



/* C++ utility function */

/* example of genertaing absolute path
    std::string w2crs_path("../module/LaneFinder/lib/cf_tracking/src/cf_libs/common/w2crs.txt");
	std::string cpp_path = __FILE__;
    cpp_path: /Users/leon/Gallopwave/Project_LaneFinder/LaneFinder/lib/cf_tracking/src/cf_libs/common/cn_feature.cpp
	std::string desir_dir = cpp_path.substr(0, cpp_path.find_last_of("/"));
	std::string w2crs_path = desir_dir + "/w2crs.txt";
 */
template<typename T>
inline void showMinMax() 
{
   std::cout << "sizeof(): " << sizeof(T) << std::endl;
   std::cout << "min: " << std::numeric_limits<T>::min() << std::endl;
   std::cout << "max: " << std::numeric_limits<T>::max() << std::endl;
   std::cout << "Test max: " << pow(2, sizeof(T) * 8 - 1) - 1 << std::endl;
   std::cout << std::endl;
}

std::string fixed_precision(std::string str_fnum, int digits = 2);
void show_elasped_time_Info(std::vector<float>& vec_elasped_time);
void progress_info(int running_time, int start_time, int last_running_time);

template <typename T>
inline float get_elapsed_time(std::chrono::steady_clock::time_point& start) 
{
    auto end = std::chrono::steady_clock::now();
    return std::chrono::duration_cast<T>(end-start).count();
}
inline std::string setw_num_gen(int fill_num, int frame_index, const std::string& suffix = ".jpg") 
{
        std::ostringstream ostr;
		ostr << std::setfill('0') << std::setw(fill_num) << frame_index << suffix;
        return ostr.str();
}
	

/* OpenCV utility function */
bool isRotationMatrix(const cv::Mat& R);
void Video_Info(const cv::VideoCapture& cap);

/* Return colored gray image(src) based on assigned ColormapTypes */
cv::Mat sudo_color(const cv::Mat& src, const cv::ColormapTypes& type = cv::COLORMAP_JET);
void Point2fToxy(const std::vector<cv::Point2f>& pts, 
                 std::vector<float>& vec_x, std::vector<float>& vec_y);

cv::Mat compute_gradient(cv::Mat src, cv::Mat& grad_mag, cv::Mat& grad_ori, 
                         float scale_factor = 1, int ksize = 3);

/* Draw Rotated Rectangle on img */
void drawRotatedRect(cv::Mat& img, const cv::RotatedRect& rotatedRectangle, const cv::Scalar& color, int thickness = 2);

template<typename T>
inline void draw_crossedRect(cv::Mat &img, const cv::Rect_<T> &Rectangle, const cv::Scalar &color, int thickness = 2) 
{
    cv::rectangle(img, Rectangle, color, thickness);
    cv::line(img, Rectangle.tl(), Rectangle.br(), color, thickness);
    auto &&tr = Rectangle.tl()+ cv::Point_<T>(Rectangle.width, 0);
    auto &&bl = Rectangle.tl()+ cv::Point_<T>(0, Rectangle.height);
    cv::line(img, tr, bl, color, thickness);
}


template<typename T1, typename T2>
inline float cal_IOU(const cv::Rect_<T1> &a, const cv::Rect_<T2> &b) 
{
    auto &&OR_rect  = a | b;
    auto &&AND_rect = a & b;    
    return (AND_rect.area() * 1.0 / OR_rect.area());
}

cv::Mat get_Gaussian_kernel(int kernel_sz, float sigma = 1);

/** 
 * Convert theta of non-oriented line based on symmetric property
 *  - Input range [-180, 180]
 *  - Converted range [0 180)                  
 */
inline void line_symmetric_angle(float &theta) { if ( theta<=0 ) { theta += 180;} }

template<typename T>
inline float cal_tau(const cv::Rect_<T> &rect) 
{
    return static_cast<float>(rect.height) / rect.width;
}

inline cv::Scalar label_color(int label_index, int total_label) 
{
    static cv::Mat Map(cv::Size(256,1), CV_8UC1);    
    static cv::Mat colorMap;
    auto data_ptr = Map.ptr<uchar>(0);
    for (size_t i = 0; i < 256; ++i)
        data_ptr[i] = i;
    cv::applyColorMap(Map, colorMap, cv::COLORMAP_RAINBOW);
    return colorMap.at<cv::Vec3b>(0, 255.0 / total_label * label_index);
}

template< typename T>
inline bool out_of_Img(const cv::Size &size, T col, T row, int offset = 0)
{
    if ( col > offset && row > offset &&
         col < (size.width  - 1 - offset) && 
         row < (size.height - 1 - offset) ) {
        return false;
    }
    return true;
}

template< typename T>
inline bool out_of_Img(const cv::Size &size, const cv::Point_<T> &pt, int offset = 3)
{
   return out_of_Img(size, pt.x, pt.y, offset);
}

template<typename T>
cv::Rect_<T> scaling_rect(float scale_factor, const cv::Rect_<T> &rect, const cv::Size &sz) {
    float &&width  = rect.width  * scale_factor;
    float &&height = rect.height * scale_factor;
    cv::Point2f shifted_org(rect.x - ((width-rect.width)   / 2),
                            rect.y - ((height-rect.height) / 2));
    cv::Rect_<T> scaled_rect = cv::Rect_<T>(shifted_org, cv::Size(width, height));
   
    shifted_org.x = (shifted_org.x < 0) ? 0 : shifted_org.x;
    shifted_org.y = (shifted_org.y < 0) ? 0 : shifted_org.y;
    scaled_rect = cv::Rect_<T>(shifted_org, scaled_rect.br());

    cv::Point2f shifted_br = scaled_rect.br();
    if (scaled_rect.br().x >= sz.width) {
        shifted_br.x = sz.width-1;
    }
    if (scaled_rect.br().y >= sz.height) {
        shifted_br.y = sz.height-1;
    }
    scaled_rect = cv::Rect_<T>(shifted_org, shifted_br);

    return scaled_rect;
}

inline void save_tmp_img(cv::Mat img, int i) {
    std::string name = "tmp_" + std::to_string(i) + ".jpg";
    cv::imwrite(name.c_str(), img );
}

inline void draw_vertex(cv::Mat &img, std::vector<cv::Point2f> &pts, cv::Scalar &color) {
    const size_t pt_num = pts.size();
    for (size_t i = 0; i < pt_num; ++i) {
        cv::circle(img,pts[i],3,GREEN,-1);
        cv::line(img, pts[i], pts[(i+1)%pt_num], color, 1);
    }
}

inline void extract_endPts(const std::vector<cv::Point2f>& undistorted_pts, cv::Point2f& endPt1, cv::Point2f& endPt2) {
    if (undistorted_pts.size()!=4)
        ERROR_MSG_EXIT("undistorted_pts.size()!=4");
    endPt1 = endPt2 = cv::Point2f(0,0);
    endPt1 = undistorted_pts[0];
    float dist = 1000000;
    int index = 0;
    for (int i = 1; i < 4; ++i) {
        auto cur_dist = euclideanDist(endPt1, undistorted_pts[i]);
        if ( cur_dist < dist) {
            dist = cur_dist;
            index = i;
        }
    }
    endPt1 += undistorted_pts[index];
    endPt1 /= 2;

    for (int i = 1; i < 4; ++i) {
        if (i == index)
            continue;
        endPt2 += undistorted_pts[i];
    }
    endPt2 /= 2;
}



/* Eigen utility function */


/* Calculate Rotation matrix given euler angles
 *  - rotation order: z(roll),y(yaw),x(pitch)
 *  -  Input unit: deg                         */
Eigen::Matrix3f euler2RotationMat(const float roll, const float yaw, const float pitch);


/* Calculate euler angles given Rotation matrix
 *  - rotation order: z(roll),y(yaw),x(pitch)
 *  - Output unit: deg                         */
void RotationMat2euler(const Eigen::Matrix3f& R, float& roll, float& yaw, float& pitch);

/* Calculate the cofficients of Polynomial regression  
 * - return X_(order+1)x1: [a0, a1, a2,...,a_order]^T
 * - representd by y = a0 + a1*x + a2*x^2 + ... + a_order*x^(order)
 * - [Ax = b]: x is equation coefficient(a0,a1,a2...)
 * - A and b are constructed by vec_x and vec_y respectively       */ 
Eigen::MatrixXf Polynomial_regression(const int order, 
                                      const std::vector<float>& vec_x, 
                                      const std::vector<float>& vec_y, 
                                      const std::vector<float>& weights = std::vector<float>());

/* Calculate the cofficients [a0, a1]^T for y = a0 + a1*x */
inline Eigen::MatrixXf line_regression(const std::vector<float>& vec_x, 
                                       const std::vector<float>& vec_y, 
                                       const std::vector<float>& weights = std::vector<float>()) {
    return Polynomial_regression(1, vec_x,vec_y, weights);
}


Eigen::MatrixXf line_regression(const std::vector<cv::Point2f>& pts,
                                const std::vector<float>& weights = std::vector<float>());

void Gaussian_MLE(const Eigen::MatrixXf& data, Eigen::MatrixXf& mean, Eigen::MatrixXf& var);
void Gaussian_MLE_parallel(const Eigen::MatrixXf& data, Eigen::MatrixXf& mean, Eigen::MatrixXf& var);
float Gaussian_PDF(const Eigen::MatrixXf& sample, const Eigen::MatrixXf& mean, const Eigen::MatrixXf& var);

inline void draw_Polynomial_curve(const Eigen::MatrixXf& lane_coeff, const cv::Point2f& view_range, cv::Mat& img, const cv::Scalar& Color, int lineWidth = 1) {
    /* [ IMPORTANT NOTE ]
     * lane_coeff is representd by x = a0 + a1*y + a2*y^2 + ....
     * the order of lane_coefff is determined by lane_coeff.rows()-1
     */
    int n = 100;
    std::vector<cv::Point2f> pointList;
    pointList.reserve(n);

    for (int i=0 ; i<n; i++) {
        cv::Point2f p(0,0);
        p.y = static_cast<float>(img.rows-1)/n*i;

        if (view_range != cv::Point2f(0,0)) {
            if (p.y<view_range.x)
                continue;
            if (p.y>view_range.y)
                break;
        }

        for (int row = 0; row<lane_coeff.rows(); row++) {
            p.x += lane_coeff(row,0)*std::pow(p.y,row);
        }
        pointList.push_back(p);
    }
    // connect each pt of pointList
    if (pointList.size()>1) {
        for (size_t i=0; i<pointList.size()-1; i++) {
            cv::line(img,pointList[i],pointList[i+1],Color,lineWidth);
        }    
    }
}


inline void draw_Polynomial_curve(const Eigen::MatrixXf& line_coeff, cv::Mat& img, const cv::Scalar& Color, int lineWidth = 1) {
    /* [ IMPORTANT NOTE ]
     * line_coeff is representd by x = a0 + a1*y + a2*y^2 + ....
     * the order of lane_coefff is determined by line_coeff.rows()-1
     */

    draw_Polynomial_curve(line_coeff, cv::Point2f(0,0), img, Color, lineWidth);
}

inline Eigen::MatrixXf line_regression(const cv::Point2f& pt1, const cv::Point2f& pt2) {
    std::vector<float> vec_x, vec_y;
    vec_x.clear(); vec_y.clear();
    vec_x.push_back(pt1.x); vec_x.push_back(pt2.x);
    vec_y.push_back(pt1.y); vec_y.push_back(pt2.y);
    return Polynomial_regression(1, vec_x, vec_y);
}



inline void draw_line(const cv::Point2f &line_coeff, cv::Mat& img, const cv::Scalar &Color, int lineWidth = 1, int upper_limit = 0) {
    cv::Point2f p1, p2;

    auto cal_x = [&](float y) {
        return (y-line_coeff.x)/line_coeff.y;
    };
    p2.y = img.rows - 1;
    p2.x = cal_x(p2.y);
    p1.y = 0;
    if (upper_limit > 0 && upper_limit < img.rows-1) { p1.y = upper_limit; }
    p1.x = cal_x(p1.y);
    cv::line(img, p1, p2, Color, lineWidth);
}


inline int colck_wise_order2phase_order2(int i) {
    int clock_wise_order;
    if (i == 1 ) {
        clock_wise_order = 2;
    }
    else if (i == 2 ) {
        clock_wise_order = 1;
    }
    else if (i == 3 ) {
        clock_wise_order = 4;
    }
    else if (i == 4 ) {
        clock_wise_order = 3; 
    }
    return clock_wise_order;
};


inline int clock_wise_order(const cv::Point2f& pt, const cv::Size& search_reg_sz) {
    cv::Point center(search_reg_sz.width/2, search_reg_sz.height/2);
    int index = 0;
    if (pt.x < center.x && pt.y < center.y) {
        index = 1;
    } else if (pt.x < center.x && pt.y >= center.y) {
        index = 4;
    } else if (pt.x >= center.x && pt.y >= center.y) {
        index = 3;
    } else if (pt.x >= center.x && pt.y < center.y) {
        index = 2;
    } else if (index == 0) {
        std::stringstream ss;
        ss << "Invalid pt: " << pt;
        ERROR_MSG_EXIT(ss.str());
    }
    return index;
};

}
#endif //UTILITY_HPP














